//
// Created by yunnan xu on 2020/12/8.
//

#include "PhyScene.h"
#include "RigidBody.h"
#include "GJK.h"
#include "Model.h"
#include "EngineCore.h"

namespace Phy
{
    bool has_collision = false;

    PhyScene::PhyScene()
    {

    }

    void PhyScene::AddRigidBody(std::shared_ptr<RigidBody> rb)
    {
        rb_list.emplace_back(rb);
    }

    void PhyScene::Update(double elapsed_time, double tot_time)
    {
        if(has_collision) return;
        for (auto & rb : rb_list)
        {
            if (rb) {
                rb->Update(elapsed_time);
            }
        }

        if (rb_list.size() >= 2)
        {

            auto rb1 = rb_list[0];
            auto rb2 = rb_list[1];

            if (rb1 && rb2) {
                Phy::GJK::CollisionInfo c_info = Phy::GJK::EPACollision(rb1, rb2);

                if (c_info.collision) {
                    LOGI("EPA Collision !!!!!! pos1 = (%f, %f, %f), pos2 = (%f, %f, %f), norm = (%f, %f, %f)",
                         c_info.rb1_pos.x, c_info.rb1_pos.y, c_info.rb1_pos.z,
                         c_info.rb2_pos.x, c_info.rb2_pos.y, c_info.rb2_pos.z,
                         c_info.normal.x, c_info.normal.y, c_info.normal.z
                    );
                    has_collision = true;
                }
            }
        }
    }
}
